#include "ControlMethod.h"

void f_outback()
{
  if (isConfine_Gray() & l_out) //左前出界
  {
    while (isConfine_Gray() & l_out)
    {
      turnRound_R(EXHIGHSPEED);
    }
  }

  else if (isConfine_Gray() & r_out) //右前出界
  {
    while (isConfine_Gray() & r_out)
    {

      turnRound_L(EXHIGHSPEED);
    }
  }

  else if ((analogRead(LEFT_GRAY) / 1023.0) < (analogRead(RIGHT_GRAY) / 1023.0)) //左侧距离边界近
  {
    while (isConfine_Gray() & (f_out))
    {
      turnRound_R(EXHIGHSPEED);
    }
  }
  else //右侧距离边界近
    while (isConfine_Gray() & (f_out))
    {
      turnRound_L(EXHIGHSPEED);
    }
}

void b_outback()
{

  while (isConfine_Gray() & b_out)
  {
    Forward(MIDSPEED);
  }
}

void l_outback()
{
    turnRight_FW(MIDSPEED,MID_OFFSET);
    delay(TIME);
}

void r_outback()
{
  
    turnLeft_FW(MIDSPEED,MID_OFFSET);
    delay(TIME);
  
}
